﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using FarseerPhysics;
using Microsoft.Xna.Framework;
using FarseerPhysics.Dynamics;
using Polar.AI.Common;
using Polar.AI.Analysis;


namespace Polar.AI.Steering
{
    /// <summary>
    /// TODO need to reorganize this code.  These static libraries are here just to test the individual functions.
    /// </summary>
    public static class SteeringQueries
    {

        #region Flock Queries...
        /// <summary>
        /// Filter the given flock to only bodies that are NOT behind us.
        /// </summary>
        public static IBody[] FindNeighborsInFront(IBody source, IBody[] flock)
        {
            List<IBody> list = new List<IBody>();

            for (int i = 0; i < flock.Length; i++)
            {
                if (SteeringQueries.IsBehind(source, flock[i].Position) == false)
                    list.Add(flock[i]);
            }

            return list.ToArray();
        }

        /// <summary>
        /// Returns the spacially closest body from a flock.
        /// </summary>
        public static IBody FindNearestNeighbor(IBody source, IBody[] flock)
        {
            float minDistance = float.MaxValue;
            IBody closestNeighbor = null;
            for (int i = 0; i < flock.Length; i++)
            {
                if (flock[i] != source)
                {
                    float offet = (flock[i].Position - source.Position).LengthSquared();
                    if (offet < minDistance)
                    {
                        minDistance = offet;
                        closestNeighbor = flock[i];
                    }
                }
            }

            return closestNeighbor;
        }
        #endregion

        #region Orientation Methods...
        /// <summary>
        /// Determines if a position is in front of a body using a default cos threshold.
        /// </summary>
        public static bool IsAhead(IBody source, Vector2 target)
        {
            return IsAhead(source, target, Calc.Cos45);
        }

        /// <summary>
        /// Determines if a position is beside of a body using a default cos threshold.
        /// </summary>
        public static bool IsBeside(IBody source, Vector2 target)
        {
            return IsBeside(source, target, Calc.Cos45);
        }

        /// <summary>
        /// Determines if a position is behind of a body using a default cos threshold.
        /// </summary>
        public static bool IsBehind(IBody source, Vector2 target)
        {
            return IsBehind(source, target, -Calc.Cos45);
        }

        /// <summary>
        /// Determines if a target position is in front of a body.  In front is defined by cos threshold.
        /// </summary>
        public static bool IsAhead(IBody source, Vector2 target, float cosThreshold)
        {
            Vector2 targetDirection = (target - source.Position);
            targetDirection.Normalize();
            return Vector2.Dot(source.Forward(), targetDirection) > cosThreshold;
        }

        /// <summary>
        /// Determines if a target position is beside of a body.  Is beside is defined by cos threshold.
        /// </summary>
        public static bool IsBeside(IBody source, Vector2 target, float cosThreshold)
        {
            Vector2 targetDirection = (target - source.Position);
            targetDirection.Normalize();
            float dp = Vector2.Dot(source.Forward(), targetDirection);
            return (dp < cosThreshold) && (dp > -cosThreshold);
        }

        /// <summary>
        /// Determines if a target position is behind of a body.  Is behind is defined by cos threshold.
        /// </summary>
        public static bool IsBehind(IBody source, Vector2 target, float cosThreshold)
        {
            Vector2 targetDirection = (target - source.Position);
            targetDirection.Normalize();
            return Vector2.Dot(source.Forward(), targetDirection) < cosThreshold;
        }

        /// <summary>
        /// Determines if a position is within a given circle.
        /// </summary>
        public static bool IsPositionInCircle(Vector2 position, Vector2 center, float radius)
        {
            //take distance from center and see if it is shorter then radius
            radius = radius * radius;
            Vector2 distance = center - position;

            return distance.LengthSquared() <= radius;
        }

        /// <summary>
        /// Determines if a body is within a minimum distance or within a certain max angle from body.
        /// </summary>
        public static bool IsInBodyNeighborhood(IBody source, IBody other, float minDistance, float maxDistance, float cosMaxAngle)
        {
            if (other == source)
            {
                return false;
            }
            else
            {
                Vector2 offset = other.Position - source.Position;
                float distanceSquared = offset.LengthSquared();

                // definitely in neighborhood if inside minDistance sphere
                if (distanceSquared < (minDistance * minDistance))
                {
                    return true;
                }
                else
                {
                    // definitely not in neighborhood if outside maxDistance sphere
                    if (distanceSquared > (maxDistance * maxDistance))
                    {
                        return false;
                    }
                    else
                    {
                        // otherwise, test angular offset from forward axis
                        Vector2 unitOffset = offset / (float)Math.Sqrt(distanceSquared);
                        float forwardness = Vector2.Dot(source.Forward(), unitOffset);
                        return forwardness > cosMaxAngle;
                    }
                }
            }
        }

        #endregion

        #region Prediction Queries...
        /// <summary>
        /// Returns a threat analysis on the body closest to colliding, limited to a certain avoid distance.
        /// </summary>
        public static ThreatAnalysis PredictNearestBodyCollision(IBody source, IBody[] flock, float minAvoidDistance)
        {
            
            ThreatAnalysis nearestThreat = null;
            float intersectDistance = -1;

            //search for the closest theat if any.
            for (int i = 0; i < flock.Length; i++)
            {
                if (source == flock[i])  //ignore me
                    continue;

                ThreatAnalysis threat = new ThreatAnalysis(source, flock[i]);
                if (threat.CollisionVector.Collision == true && threat.CollisionVector.IntersectDistance < minAvoidDistance) //threat within our min avoidance 
                {
                    if (intersectDistance.EqualTo(-1) || intersectDistance > threat.CollisionVector.IntersectDistance)  //record closest threat
                    {
                        nearestThreat = threat;
                    }
                }
            }

            return nearestThreat;
        }
        #endregion
    }
}
